﻿using System;
using System.Collections.Generic;
using System.Text;
using G3.State;
using G3.Controller;
using dnAnalytics.LinearAlgebra;
using dnAnalytics;
using System.IO;

namespace G3.AI
{
    [Serializable]
    class UniversalNeuralDriver:IDriver
    {
        Controller.Controller myController;
        Matrix transformation;
        Vector lastResponse;
        //const double LEARNINGFACTOR = 0,5;
        Vector inputVector = VectorBuilder.CreateVector(int.Parse((String)Parameters.GetParameter("FieldOfVisionHeight")) *
                (int.Parse((String)Parameters.GetParameter("FieldOfVisionWidth")))
                + 4);
        const double TIME_STEP = 0.07;

        //StreamWriter log = new StreamWriter("universalAITeach.log");           

        public UniversalNeuralDriver()
        {
            myController = G3.Controller.Controller.GetOnlyInstance();
            int matrixSize = ((int.Parse((String)Parameters.GetParameter("FieldOfVisionHeight"))) *
                (int.Parse((String)Parameters.GetParameter("FieldOfVisionWidth"))));
            transformation = MatrixBuilder.CreateMatrix(matrixSize + 4, 2);

            for (int i = 0; i < transformation.Rows; ++i)
                for (int j = 0; j < transformation.Columns; ++j)
                {
                    if (j == 0)
                    {
                        transformation[i, j] = Math.Pow(-1,i)* 0.01;
                    }
                    else
                    {
                        transformation[i, j] = 0.1;
                    }
                    
                }
            //transformation[transformation.Rows - 2, 1] = 10;            
            lastResponse = VectorBuilder.CreateVector(2);
        }

        public Pair Think()
        {
            double carSpeed;
            KeyValuePair<Pair, double> carPosOrientation;

            carSpeed = myController.GetCarSpeed();
            carPosOrientation = myController.GetCarPositionAndAngle();
            int[,] fov = Map.GetFieldOfVision(carPosOrientation.Key, carPosOrientation.Value);
            
            
            for(int i=0; i<fov.GetLength(0); ++i)
                for (int j = 0; j < fov.GetLength(1); ++j)
                {
                    inputVector[i * fov.GetLength(0) + j] = fov[i, j];
                }


            inputVector[fov.Length] = carSpeed;
            Field f = Map.GetSurface((int)Math.Round(carPosOrientation.Key.x),
                (int)Math.Round(carPosOrientation.Key.y));
            inputVector[fov.Length + 1] = f.friction;
            inputVector[fov.Length + 2] = f.grip;
            inputVector[fov.Length + 3] = f.penalty;

            Vector outputVector = VectorBuilder.CreateVector(2);
            outputVector = inputVector * transformation;
            //wheel
            outputVector[0] = Truncate(-100, 100, (float)outputVector[0]);
            //throotle
            outputVector[1] = Truncate(-100, 100, (float)outputVector[1]);

            myController.SetWheelThrootlePositions(outputVector[1], outputVector[0]);
            lastResponse = outputVector;
            return new Pair(outputVector[0], outputVector[1]);
            return new Pair(0, 0);
        }

        public void Teach(int rounds)
        {
            myController.AttachedState.Reset();
            Think();
            myController.AttachedState.Simulate(TIME_STEP);
            Think();
            myController.AttachedState.Simulate(TIME_STEP);
            for (long i = 0; i < rounds; ++i)
            {                
                Think();
                myController.AttachedState.Simulate(TIME_STEP);
                TeachIteration();
                myController.ShowOnMap();
            }



//            log.WriteLine(transformation.ToString());
  //          log.Close();
        }

        private void TeachIteration()
        {
            State.State currentState = Controller.Controller.GetOnlyInstance().AttachedState;
            Pair optPos = Controller.Map.GetOptimalPosition();
            State.State.StateVars currentSV = currentState.GetStateVars(-1);
            Pair optimalResponsePair = currentState.ArcSim(
                currentSV,new State.State.StateVars(Map.MapCoordsToStateCoords(optPos),                          
                    new Pair(0, 0),                                                            
                    inputVector[inputVector.ToArray().Length-4]),
                    TIME_STEP);

            Pair carPos=Map.StateCoordsToMapCoords(myController.AttachedState.CarPosition);
//log.WriteLine("currentPos:\t" + carPos.x + "\t" + carPos.y);
//log.WriteLine("optimalPos:\t" + optPos.x + "\t" + optPos.y);
//log.WriteLine("optimalResponsePair:\t" + optimalResponsePair.x + "\t" + optimalResponsePair.y + "\n");
            Vector optimalResponse = VectorBuilder.CreateVector(2);
            //kormány
            if (!double.IsNaN(optimalResponsePair.x) && !double.IsNaN(optimalResponsePair.y))
            {
                optimalResponse[0] = Truncate(-100, 100, optimalResponsePair.x);
                //gáz
                optimalResponse[1] = Truncate(-100, 100, optimalResponsePair.y);
            }

            optimalResponse[1] = 30;

            Vector error = VectorBuilder.CreateVector(2);
            optimalResponse.Subtract(lastResponse, error);
            
            for (int i = 0; i != transformation.Rows; ++i)
            {
                error.Multiply(0.5);
                Vector delta = inputVector[i] * error;
                transformation.SetRow(i, (delta + transformation.Row(i)));
            }            
        }        

        public DriverType Type()
        {
            return DriverType.Neural;
        }

        private double Truncate(double lowerBound, double upperBound, double value)
        {
            if (value < lowerBound || double.IsNegativeInfinity(value))
                return lowerBound;
            else if (value > upperBound || double.IsPositiveInfinity(value))
                return upperBound;
            else if (double.IsNaN(value))
                throw new Exception("végtelen!?!");
            return value;
        }

        public void LoadProfile(string filePath)
        {
            
        }

        public void SaveProfile(string filePath)
        {
        }
    }
}
